// Author: 	Asil Kaan Bozcuoglu
// Date: 	Apr 22 2011
// Point Cloud transformation using ROS

#include <iostream>
#include "fstream"
#include "sstream"
#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include "pcl_ros/io/bag_io.h"

#include "sys/stat.h"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_ros/transforms.h>
#define MAX_SAMPLE_INDEX 10000

using namespace pcl;
using namespace pcl::io;
using namespace std;

int main(int argc, char* argv[])
{
	string inPath;
	string outPath;
	
	cout	<<"Enter the path of the input cloud"<<endl;
    	cin>>inPath;

	cout	<<"Enter the path of the output cloud"<<endl;
    	cin>>outPath;

	ifstream inFile;
	inFile.open(inPath.c_str());
	ofstream outFile;
	outFile.open(outPath.c_str());

	pcl::PointCloud<pcl::PointXYZ> cloud;
	//pcl::PointCloud<pcl::PointXYZ> cloud2;
	sensor_msgs::PointCloud2 cloud2;
	sensor_msgs::PointCloud2 cloud3;
	pcl::PointCloud<pcl::PointXYZ> cloud4;

	cloud.header.frame_id = "/";
	cloud.width    = 176;
  	cloud.height   = 144;
  	cloud.is_dense = false;
  	cloud.points.resize (176 * 144);
	for(int j=0;j<144*176;j++)
	{
		inFile >> cloud.points[j].x;
		inFile >> cloud.points[j].y;
		inFile >> cloud.points[j].z;
	}

	/*if (*/pcl::toROSMsg(cloud, cloud2);/*)
		cout	<<"PointCloud conversion is successful!"<<endl;
	else
		cout	<<"PointCloud conversion is failed!"<<endl;*/
	
	Eigen::Matrix4f out_mat;
	
	out_mat (0, 0) = 1; out_mat (0, 1) = 0; out_mat (0, 2) = 0;
	out_mat (1, 0) = 0; out_mat (1, 1) = 0; out_mat (1, 2) = -1;
	out_mat (2, 0) = 0; out_mat (2, 1) = 1; out_mat (2, 2) = 0;
                                                                      
   	out_mat (3, 0) = 0; out_mat (3, 1) = 1; out_mat (3, 2) = 0; out_mat (3, 3) = 1;
	out_mat (0, 3) = 1;
	out_mat (1, 3) = 1;
	out_mat (2, 3) = 1;	
	pcl_ros::transformPointCloud(out_mat, cloud2, cloud3);
	pcl::fromROSMsg(cloud3, cloud4);
	for(int j=0;j<144*176;j++)
	{
		outFile << cloud4.points[j].x << " ";
		outFile << cloud4.points[j].y << " ";
		outFile << cloud4.points[j].z << endl;
	}

	inFile.close();
	outFile.close();

	return 0;
}
